{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PreintegratedCombinedMeasurements\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegratedCombinedMeasurements.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `PreintegratedCombinedMeasurements` class is the preintegration container specifically designed for use with the `CombinedImuFactor`. Like `PreintegratedImuMeasurements`, it accumulates IMU measurements between two time steps ($t_i, t_j$) using a fixed bias estimate (`biasHat_`).\n",
    "\n",
    "However, it differs significantly in its covariance propagation:\n",
    "1.  It uses `PreintegrationCombinedParams`, which include bias random walk parameters.\n",
    "2.  It propagates a larger **15x15 covariance matrix** (`preintMeasCov_`). This matrix captures the uncertainty of the preintegrated $\\Delta R, \\Delta p, \\Delta v$ (9x9 block), the uncertainty of the bias estimate itself (6x6 block), and crucially, the **cross-correlations** between the preintegrated measurements and the bias estimate.\n",
    "\n",
    "Accounting for these correlations and the bias evolution noise allows the `CombinedImuFactor` to properly model the relationship between the states at $t_i, t_j$ and the biases at $b_i, b_j$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
      "\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
      "\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Background\n",
    "\n",
    "The mean propagation (calculating $\\Delta R, \\Delta p, \\Delta v$) is mathematically identical to that in `PreintegratedImuMeasurements`, using the same underlying implementation (`ManifoldPreintegration` or `TangentPreintegration`).\n",
    "\n",
    "The key difference lies in the covariance propagation. The update step for the 15x15 covariance matrix $\\Sigma_k \\rightarrow \\Sigma_{k+1}$ incorporates not only the IMU measurement noise (like the standard PIM) but also:\n",
    "- The noise from the bias random walk process (defined in `PreintegrationCombinedParams`).\n",
    "- The uncertainty of the `biasHat_` used during the integration step (via `biasAccOmegaInit` from the parameters).\n",
    "\n",
    "This results in a more complex but statistically more accurate propagation of uncertainty, especially when biases are expected to drift significantly or have substantial initial uncertainty. The derivation details can be found in technical reports and papers related to the `CombinedImuFactor` (e.g., Carlone et al., IJRR 2015)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "The API is very similar to `PreintegratedImuMeasurements`:\n",
    "\n",
    "- **Constructor**: `PreintegratedCombinedMeasurements(params, biasHat)`: Requires shared `PreintegrationCombinedParams`.\n",
    "- **`integrateMeasurement(measuredAcc, measuredOmega, dt)`**: Adds a measurement, updating the internal state and the 15x15 covariance.\n",
    "- **`resetIntegration()` / `resetIntegrationAndSetBias(newBiasHat)`**: Resets the integration.\n",
    "- **Accessors**: `deltaTij()`, `deltaRij()`, `deltaPij()`, `deltaVij()`, `biasHat()`. \n",
    "- **`preintMeasCov()`**: Returns the **15x15** covariance matrix.\n",
    "- **`predict(state_i, current_bias)`**: Predicts state $X_j$ (same logic as standard PIM, using only the $\\Delta R, p, v$ parts).\n",
    "- **`biasCorrectedDelta(current_bias)`**: Returns the 9D tangent-space vector corrected for bias difference (same logic as standard PIM).\n",
    "- **Note**: There isn't a direct equivalent of `computeError` within this class, as the full error calculation (including the bias random walk part) is handled by the `CombinedImuFactor` itself."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Create combined parameters, create the object, integrate measurements."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total integration time: 0.09999999999999999\n",
      "Delta R:\n",
      " [[ 0.9999875  -0.00499998  0.        ]\n",
      " [ 0.00499998  0.9999875   0.        ]\n",
      " [ 0.          0.          1.        ]]\n",
      "Delta P: [ 4.99999147e-04  7.12499187e-07 -4.90000000e-02]\n",
      "Delta V: [ 9.99996438e-03  2.24999578e-05 -9.80000000e-01]\n",
      "Preintegration Covariance (15x15 shape): (15, 15)\n"
     ]
    }
   ],
   "source": [
    "from gtsam import PreintegrationCombinedParams, PreintegratedCombinedMeasurements\n",
    "from gtsam.imuBias import ConstantBias\n",
    "import numpy as np\n",
    "\n",
    "# 1. Create Combined Parameters (as in PreintegrationCombinedParams example)\n",
    "params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
    "accel_noise_sigma = 0.1\n",
    "gyro_noise_sigma = 0.01\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "bias_acc_rw_sigma = 0.001\n",
    "bias_gyro_rw_sigma = 0.0001\n",
    "params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
    "params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
    "initial_bias_cov = np.diag(np.full(6, 1e-3)) # Example initial bias uncertainty\n",
    "params.setBiasAccOmegaInit(initial_bias_cov)\n",
    "\n",
    "# 2. Define the bias estimate used for preintegration\n",
    "bias_hat = ConstantBias() # Start with zero bias estimate\n",
    "\n",
    "# 3. Create the PreintegratedCombinedMeasurements object\n",
    "pim = PreintegratedCombinedMeasurements(params, bias_hat)\n",
    "\n",
    "# 4. Integrate measurements\n",
    "dt = 0.01 # 100 Hz\n",
    "num_measurements = 10\n",
    "acc_meas = np.array([0.1, 0.0, -9.8]) \n",
    "gyro_meas = np.array([0.0, 0.0, 0.05])\n",
    "\n",
    "for _ in range(num_measurements):\n",
    "    pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
    "\n",
    "# 5. Inspect the results\n",
    "print(\"Total integration time:\", pim.deltaTij())\n",
    "print(\"Delta R:\\n\", pim.deltaRij().matrix())\n",
    "print(\"Delta P:\", pim.deltaPij())\n",
    "print(\"Delta V:\", pim.deltaVij())\n",
    "print(\"Preintegration Covariance (15x15 shape):\", pim.preintMeasCov().shape)\n",
    "# print(\"Preintegration Covariance:\\n\", pim.preintMeasCov()) # Might be large"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This `pim` object is then passed to the `CombinedImuFactor` constructor."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [CombinedImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h) (Contains definition)\n",
    "- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
